#include "componentCheck.h"

bool presenceCorrect(Component source, Component target, float threshold)
{
    int minimumNumberOfPoints = target.getIndices()->indices.size() * (threshold)/100;
    float scoreThreshold = threshold;
    //cout<<"PRESENCE[num of points]: Source="<< source.getIndices()->indices.size()<<" - Target="<< target.getIndices()->indices.size();
    float score=validationScoreProximityTarget(* source.getCloud(), * target.getComponentCloud());
    cout <<"PRESENCE[fitness score]: Score: "<<score <<" Threshold: "<<scoreThreshold;
    if (score<scoreThreshold)//METODO CONTA PUNTI((source.getIndices()->indices.size()>=minimumNumberOfPoints)
    {
        cout << " ==> OK!" <<endl;
        return true;
    }
    else
    {
        cout << " ==> ERROR!" <<endl;
        return false;
    }
}

bool positionCorrect(Component source, Component target, Eigen::Matrix4f transformation, float threshold)
{

    Eigen::Vector3f translation = source.getCenterOfMass().getVector3fMap()- target.getCenterOfMass().getVector3fMap();
    cout<< "POSITION[mm]: Difference = "<< translation.norm() << " - Threshold = " << threshold;

    if (translation.norm() < threshold)
    {
        cout << " ==> OK!" <<endl;
        return true;
    }
    else
    {
        cout << " ==> ERROR!" <<endl;
        return false;
    }
}

bool inclinationCorrect(Component source, Component target, Eigen::Matrix4f transformation, float threshold)
{
//    // metodo normali/piani
//    Eigen::VectorXf sourceVector = source.getPlaneCoefficients();
//    Eigen::VectorXf targetVector = target.getPlaneCoefficients();
//    float rotation1 = rad2deg( acos(  (sourceVector.dot(targetVector)) / (sourceVector.norm() * targetVector.norm())  ) );
//    cout << "metodo normali: angolo °"<< rotation1 <<endl;
    // metodo registrazione
    Eigen::Matrix3f rotationMatrix = transformation.topLeftCorner(3,3);
    Eigen::Vector3f eulerAngles = rotationMatrix.eulerAngles(0, 1, 2);
    Eigen::Matrix3f inclinationMatrix;
    inclinationMatrix = Eigen::AngleAxisf(eulerAngles[0], Eigen::Vector3f::UnitX()) *
            Eigen::AngleAxisf(eulerAngles[1],  Eigen::Vector3f::UnitY()) *
            Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitZ());
    Eigen::AngleAxisf angleAxis(inclinationMatrix);
    //cout << "Inclination Axis : " << endl << angleAxis.axis() << endl;
    //cout << "Inclination Angle [rad] : " << angleAxis.angle() << endl; // angolo di inclinazione stimato dalla registrazione
    float rotation2 = rad2deg(angleAxis.angle());
    cout << "INCLINATION[°]: Angle = " << rotation2 << " - Threshold = " << threshold ;
    if (rotation2 < threshold)
    {
        cout << " ==> OK!" <<endl;
        return true;
    }
    else
    {
        cout << " ==> ERROR!" <<endl;
        return false;
    }
}

bool orientationCorrect(Component source, Component target, Eigen::Matrix4f transformation, float threshold)
{
    Eigen::Matrix3f rotationMatrix;
    rotationMatrix = transformation.topLeftCorner(3,3);
    Eigen::Vector3f eulerAngles = rotationMatrix.eulerAngles(0, 1, 2); // extract roll, pitch, yaw (in radians, i guess)
    //cout << "Orientation Angle [rad] : " << eulerAngles[2] << endl;
    cout << "ORIENTATION[°]: Angle = "<< abs(rad2deg(eulerAngles[2])) << " - Threshold = " << threshold;
    if(abs(rad2deg(eulerAngles[2])) < threshold)
    {
        cout << " ==> OK!" <<endl;
        return true;
    }
    else
    {
        cout << " ==> ERROR!" <<endl;
        return false;
    }
}

bool radiusCorrect(Component source, Component target, float threshold)
{
    float source_radius = source.getRadiusToCenterOfMass();
    float target_radius = target.getRadiusToCenterOfMass();
    float difference= abs(source_radius-target_radius);
    cout << "RADIUS[mm]: Source= " << source_radius << " - Target=" << target_radius << " - Difference="<< difference << " - Threshold="<< threshold;
    if (difference<threshold)
    {
        cout << " ==> OK!" <<endl;
        return true;
    }
    else
    {
        cout << " ==> ERROR!" <<endl;
        return false;
    }
}

bool heightCorrect(Component source, Component target, float threshold)
{
    float source_height = source.getAverageHeight();
    float target_height = target.getAverageHeight();
    float difference= abs(source_height-target_height);
    cout<< "HEIGHT[mm]: Source="<<source_height<<" - Target="<< target_height << " - Difference="<< difference <<" - Threshold="<< threshold ;
    if (abs(difference)<threshold)
    {
        cout << " ==> OK!" <<endl;
        return true;
    }
    else
    {
        cout << " ==> ERROR!" <<endl;
        return false;
    }
}

bool colorCorrect(Component source, Component target, float threshold)
{
    Eigen::Vector3f sourceColor = source.getAverageColor();
    Eigen::Vector3f targetColor = target.getAverageColor();
    cout<< "COLOR[rgb]: Source="<<sourceColor[0] << "/" << sourceColor[1] << "/" << sourceColor[2]
        <<" - Target="<<targetColor[0] << "/" << targetColor[1] << "/" << targetColor[2] <<" - Threshold="<<threshold ;

    Eigen::Vector3f diff = sourceColor - targetColor;
    if( diff[0] < threshold &&
            diff[0] > -threshold &&
            diff[1] < threshold &&
            diff[1] > -threshold &&
            diff[2] < threshold &&
            diff[2] > -threshold )
    {
        cout << " ==> OK!" <<endl;
        return true;
    }
    else
    {
        cout << " ==> ERROR!" <<endl;
        return false;
    }
}
